#include "AStarSearch.h"

float AStar::Heuristic(int n1, int n2)
{
	D3DXVECTOR2 vec = m_Graph.GetNode(n2)->GetPos() - m_Graph.GetNode(n1)->GetPos();
	return sqrt((vec.x*vec.x)+(vec.y*vec.y));
}
void AStar::Search()
{
	m_PQ.Push(m_Source);

	while(!m_PQ.Empty())
	{

		int node = m_PQ.Front();

		//remove top node
		m_PQ.Pop();

		//mark edge TO node as closed
		m_Closed[node] = m_Open[node];

		//if node == target  win!
		if (node==m_Target)
		{
			return;
		}

		//get list of edges FROM node
		std::list<GraphEdge> GE = m_Graph.GetEdgeList(node);
		double Hcost;
		double Gcost;

		//for each edge in edge list
		for(std::list<GraphEdge>::iterator it=GE.begin();it!=GE.end();++it)
		{
			//calc h cost
			Hcost=Heuristic((*it).GetFrom(),(*it).GetTo());

			//calc g cost
			Gcost = m_GCost[node] + it->GetCost();

			if(m_Open[(*it).GetTo()].GetTo() == -1)
			{
				//set cost
				m_GCost[it->GetTo()]=Gcost;
				m_FCost[it->GetTo()]=Gcost+Hcost;
				//add node to PQ
				m_PQ.Push(it->GetTo());
				//Set Edge to OPEN
				m_Open[(*it).GetTo()]=(*it);
			}

			//ELSE (node IS in OPEN)
			else
			{
				//new cost less then old cost
				if(Gcost<m_GCost[node])
				{
					//update cost
					m_GCost[it->GetTo()]=Gcost;
					m_FCost[it->GetTo()]=Gcost+Hcost;
					//Resort Priority Queue
					m_PQ.Resort();
					//Set OPEN list edge to this edge
					m_Open[(*it).GetTo()]=(*it);
				}
			}
		}


	}
}
std::list<int> AStar::Path()
{
	std::list<int> res;
	int cur = m_Target;
	res.push_front(m_Target);
	while (cur != m_Source)
	{
		res.push_front(m_Closed[cur].GetFrom());
		cur = res.front();
	}
	return res;
}
double AStar::GetCost()
{
	return m_FCost[m_Target];
}